//------------------------------------------------------------------------------
//
// Synchronized Dance Service - Performs a basic "dance" with multiple robots
//
// Written Feb-2008 by Trevor Taylor, Software Technology, Australia
//
// In response to postings in the MRDS Discussion Forum, here is an example
// of how to use more than one robot.
//
//------------------------------------------------------------------------------
using Microsoft.Ccr.Core;
using Microsoft.Dss.Core;
using Microsoft.Dss.Core.Attributes;
using Microsoft.Dss.ServiceModel.Dssp;
using Microsoft.Dss.ServiceModel.DsspServiceBase;
using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Xml;
using dance = ProMRDS.Robotics.SynchronizedDance;
using drive = Microsoft.Robotics.Services.Drive.Proxy;
// Added for the Fault class
using W3C.Soap;

// For locating services in the Directory
using ds = Microsoft.Dss.Services.Directory;


namespace ProMRDS.Robotics.SynchronizedDance
{
    /// <summary>
    /// Implementation class for SynchronizedDance
    /// </summary>
    [DisplayName("SynchronizedDance")]
    [Description("The Synchronized Dance Service")]
    [Contract(Contract.Identifier)]
    public class SynchronizedDanceService : DsspServiceBase
    {
        #region Constants

        // Constants for motor power and timing

        const int _repeatCount = 2;         // Number of times to repeat dance

        const int _settlingTime = 500;      // Time to wait after each move

        // Values for "exact" movements using DriveDistance and RotateDegrees
        const bool _waitForMoves = false;   // Wait for response messages
        const float _driveDistance = 0.30f; // Drive 30cm
        const float _rotateAngle = 90.0f;   // Turn 90 degrees to the left

        // Constants for the motions using various methods
        const bool _controlledMoves = false;// Use controlled (not timed) moves
        const float _drivePower = 0.5f;     // Power driving forward
        const float _rotatePower = 0.25f;   // Power during rotation

        // Values for timed moves
        const int _driveTime = 1200;        // Time to drive forward (millisec)
        const int _rotateTime = 920;        // Time to rotate

        #endregion

        /// <summary>
        /// _state - Synchronized Dance State
        /// </summary>
        [InitialStatePartner(Optional = true, ServiceUri = "SynchronizedDance.Config.xml")]
        private SynchronizedDanceState _state = new SynchronizedDanceState();

        /// <summary>
        /// _main Port - Synchronized Dance Operations Port
        /// </summary>
        [ServicePort("/SynchronizedDance", AllowMultipleInstances=false)]
        private SynchronizedDanceOperations _mainPort = new SynchronizedDanceOperations();

        #region partners

        // Partner with the generic Differential Drive service
//        [Partner("Drive", Contract = drive.Contract.Identifier, CreationPolicy = PartnerCreationPolicy.UseExisting)]
        List<drive.DriveOperations> _drivePorts = new List<drive.DriveOperations>();

        #endregion

        /// <summary>
        /// Default Service Constructor
        /// </summary>
        public SynchronizedDanceService(DsspServiceCreationPort creationPort) : 
                base(creationPort)
        {
        }

        /// <summary>
        /// Service Start
        /// </summary>
        protected override void Start()
        {
            InitializeState();

			base.Start();

            // Announce the service
            LogInfo(LogGroups.Console, "SynchronizedDance starting ...");

            SpawnIterator(Behavior);
        }


        void InitializeState()
        {
            if (_state == null)
            {
                _state = new SynchronizedDanceState();

                // Set all of the values to the defaults
                _state.CountDown = 10;                  // Number of seconds to delay at start
                _state.RepeatCount = _repeatCount;      // Number of times to repeat dance
                _state.SettlingTime = _settlingTime;    // Time to wait after each move
                _state.WaitForMoves = _waitForMoves;    // Wait for response messages
                _state.ControlledMoves = _controlledMoves;  // Use controlled (not timed) moves

                // Values for the motions using either method
                _state.DrivePower = _drivePower;        // Power for driving forward
                _state.RotatePower = _rotatePower;      // Power for rotations

                // Values for "exact" movements using DriveDistance and RotateDegrees
                // which are called "controlled" moves
                _state.DriveDistance = _driveDistance;  // Drive 30cm
                _state.RotateAngle = _rotateAngle;      // Turn 90 degrees to the left

                // Values to use with timed moves
                _state.DriveTime = _driveTime;          // Time to drive forward (millisec)
                _state.RotateTime = _rotateTime;        // Time to rotate

                // Save the state now so that the user has something to edit
                SaveState(_state);
            }

        }

        /// <summary>
        /// Get Handler
        /// </summary>
        /// <param name="get"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Concurrent)]
        public virtual IEnumerator<ITask> GetHandler(Get get)
        {
            get.ResponsePort.Post(_state);
            yield break;
        }

        #region Drive Connections

        IEnumerator<ITask> ConnectDiffDrives()
        {
            // List to return all of the services in
            ServiceInfoType[] list = null;

            // Construct a ServiceUrt that points to the local Directory service
            string host = "localhost";
            // NOTE: The port for TCP is NOT 50000!
            int port = 50001;
            UriBuilder builder = new UriBuilder(Schemes.DsspTcp, host, port, ServicePaths.InstanceDirectory);
            builder.Scheme = new Uri(ServiceInfo.Service).Scheme;

            // Get a port to talk to the Directory on
            ds.DirectoryPort dsPort = ServiceForwarder<ds.DirectoryPort>(builder.Uri);

            // Issue a Get to get a list of all services
            ds.Get get = new ds.Get();
            dsPort.Post(get);

            // Wait for the Get to complete
            yield return Arbiter.Choice(get.ResponsePort,
                delegate(ds.GetResponseType response)
                {
                    list = response.RecordList;
                },
                delegate(Fault fault)
                {
                    // Hopefully this never happens!
                    list = new ServiceInfoType[0];
                    LogError(LogGroups.Console, fault.ToString());
                }
            );

            // This should never happen, but defensive programming says ...
            if (list.Length > 0)
            {
                // Make a nice little message to let the user know what is happening
                UriBuilder node = new UriBuilder(list[0].Service);
                node.Path = null;
                string nodestring = "Found " + list.Length + " services on " +
                                    node.Host + ":" + node.Port;
                LogInfo(LogGroups.Console, nodestring);
            }
            else
            {
                LogError(LogGroups.Console, "No services found on host!");
                yield break;
            }

            foreach (ServiceInfoType info in list)
            {
                // If this service is a Diff Drive ...
                if (info.Contract == drive.Contract.Identifier)
                {
                    // Connect to the drive and add it to the list of ports
                    yield return Arbiter.ExecuteToCompletion(
                        Environment.TaskQueue,
                        new IterativeTask<string>(info.Service, ConnectDrive)
                    );
                }
            }

            // See if we got any drives
            if (_drivePorts.Count == 0)
                LogError(LogGroups.Console, "No Diff Drive services found");

            yield break;
        }

        /// <summary>
        /// Connect to a Generic Differential Drive service
        /// </summary>
        /// <param name="service"></param>
        /// <returns>Updates the global _drivePorts list</returns>
        IEnumerator<ITask> ConnectDrive(string service)
        {
            drive.DriveOperations drivePort = null;

            // Get a new drive port
            drivePort = ServiceForwarder<drive.DriveOperations>(service);

            if (drivePort != null)
            {
                // Enable the drive
                drive.EnableDriveRequest request = new drive.EnableDriveRequest();
                request.Enable = true;

                yield return Arbiter.Choice(
                    drivePort.EnableDrive(request),
                    delegate(DefaultUpdateResponseType response) { },
                    delegate(Fault f)
                    {
                        LogError(f);
                    }
                );

                // Record the service URI so the user can see it
                //_state.DriveService = service;
                LogInfo(LogGroups.Console, "Connected to Drive " + service);

                // Remember the port
                _drivePorts.Add(drivePort);

                // Subscribe to the drive for notifications?
                // Maybe in the next version ...
            }

        }

        #endregion

        #region behavior

        /// <summary>
        /// Behavior - Executes the service behavior
        /// </summary>
        /// <returns></returns>
        /// It is important to use an Iterator so that it can relinquish control
        /// when there is nothing to do, i.e. yield return
        IEnumerator<ITask> Behavior()
        {
            int i;

            // Wait for all the robots to initialize, otherwise they
            // might not appear in the list or they will miss the initial command.
            // In particular, the Simulator can take a while to start up. You might
            // even have to increase this delay.
            for (i = _state.CountDown; i > 0; i--)
            {
                LogInfo(LogGroups.Console, i.ToString());
                yield return Arbiter.Receive(
                    false,
                    TimeoutPort(1000),
                    delegate(DateTime timeout) { });
            }

            LogInfo(LogGroups.Console, "Starting now ...");

            // Connect all of the robots
            yield return Arbiter.ExecuteToCompletion(Environment.TaskQueue,
                        Arbiter.FromIteratorHandler(ConnectDiffDrives));

            // Don't continue if there are no drives
            if (_drivePorts.Count == 0)
                yield break;

            // NOTE: Drives must be enabled first!
            // This is done in ConnectDrive

            for (int times = 0; times < _state.RepeatCount; times++)
            {
                // Drive along the four sides of a square
                for (int side = 0; side < 4; side++)
                {
                    // Keep the user awake!
                    LogInfo(LogGroups.Console, "Side " + side);

                    if (_state.ControlledMoves && _state.WaitForMoves)
                    {
                        bool success = true;

                        PortSet<DefaultUpdateResponseType, Fault> p = new PortSet<DefaultUpdateResponseType, Fault>();
                        drive.DriveDistance[] driveRequests = new drive.DriveDistance[_drivePorts.Count];

                        for (i = 0; i < _drivePorts.Count; i++)
                        {
                            driveRequests[i] = new drive.DriveDistance();
                            driveRequests[i].Body.Distance = _state.DriveDistance;
                            driveRequests[i].Body.Power = _state.DrivePower;
                            driveRequests[i].ResponsePort = p;
                            _drivePorts[i].Post(driveRequests[i]);
                        }

                        yield return Arbiter.MultipleItemReceive(p, _drivePorts.Count, driveHandler);

                        /*
                        initCountHandler(_drivePorts.Count);
                        for (i = 0; i < _drivePorts.Count; i++)
                        {
                            // Receive the response whether it is success or failure
                            Activate(Arbiter.Choice(
                                _drivePorts[i].DriveDistance(_state.DriveDistance, _state.DrivePower),
                                delegate(DefaultUpdateResponseType r)
                                {
                                    counterPort.Post(1);
                                },
                                delegate(Fault f)
                                {
                                    LogError(f.ToString());
                                    counterPort.Post(-1);
                                }
                                )
                            );
                        }

                        yield return Arbiter.Receive(false, countCompletePort,
                            delegate(bool status)
                            {
                                if (!status)
                                    success = false;
                            }
                        );
                        */

                        // Wait for settling time
                        yield return Arbiter.Receive(
                            false,
                            TimeoutPort(_state.SettlingTime * 4),
                            delegate(DateTime timeout) { });

                        drive.RotateDegrees[] rotateRequests = new drive.RotateDegrees[_drivePorts.Count];

                        for (i = 0; i < _drivePorts.Count; i++)
                        {
                            rotateRequests[i] = new drive.RotateDegrees();
                            rotateRequests[i].Body.Degrees = _state.RotateAngle;
                            rotateRequests[i].Body.Power = _state.RotatePower;
                            rotateRequests[i].ResponsePort = p;
                            _drivePorts[i].Post(rotateRequests[i]);
                        }

                        yield return Arbiter.MultipleItemReceive(p, _drivePorts.Count, driveHandler);

                        /*
                        initCountHandler(_drivePorts.Count);
                        for (i = 0; i < _drivePorts.Count; i++)
                        {
                            // Receive the response whether it is success or failure
                            Activate(Arbiter.Choice(
                                _drivePorts[i].RotateDegrees(_state.RotateAngle, _state.DrivePower),
                                delegate(DefaultUpdateResponseType r)
                                {
                                    counterPort.Post(1);
                                },
                                delegate(Fault f)
                                {
                                    LogError(f.ToString());
                                    counterPort.Post(-1);
                                }
                                )
                            );
                        }

                        yield return Arbiter.Receive(false, countCompletePort,
                            delegate(bool status)
                            {
                                if (!status)
                                    success = false;
                            }
                        );
                         */

                        // Wait for settling time
                        yield return Arbiter.Receive(
                            false,
                            TimeoutPort(_state.SettlingTime * 4),
                            delegate(DateTime timeout) { });

                        if (!success)
                            LogError("Error occurred while attempting to drive robots");
                    }
                    else
                    {
                        // Start driving and then wait for a while
                        if (_state.ControlledMoves)
                            for (i = 0; i < _drivePorts.Count; i++)
                                _drivePorts[i].DriveDistance(_state.DriveDistance, _state.DrivePower);
                        else
                            for (i = 0; i < _drivePorts.Count; i++)
                                _drivePorts[i].SetDrivePower(_state.DrivePower, _state.DrivePower);

                        yield return Arbiter.Receive(
                            false,
                            TimeoutPort(_state.DriveTime),
                            delegate(DateTime timeout) { });

                        // Stop the motors and wait for robot to settle
                        for (i = 0; i < _drivePorts.Count; i++)
                            _drivePorts[i].SetDrivePower(0, 0);

                        // Wait for the settling period after a stop
                        yield return Arbiter.Receive(
                            false,
                            TimeoutPort(_state.SettlingTime),
                            delegate(DateTime timeout) { });

                        // Now turn left and wait for a different amount of time
                        if (_state.ControlledMoves)
                            for (i = 0; i < _drivePorts.Count; i++)
                                _drivePorts[i].RotateDegrees(_state.RotateAngle, _state.RotatePower);
                        else
                            for (i = 0; i < _drivePorts.Count; i++)
                                _drivePorts[i].SetDrivePower(-_state.RotatePower, _state.RotatePower);

                        yield return Arbiter.Receive(
                            false,
                            TimeoutPort(_state.RotateTime),
                            delegate(DateTime timeout) { });

                        // Stop the motors and wait for robot to settle
                        for (i = 0; i < _drivePorts.Count; i++)
                            _drivePorts[i].SetDrivePower(0, 0);

                        yield return Arbiter.Receive(
                            false,
                            TimeoutPort(_state.SettlingTime),
                            delegate(DateTime timeout) { });
                    }
                }
            }

            // And finally make sure that the robot is stopped!
            for (i = 0; i < _drivePorts.Count; i++)
                _drivePorts[i].SetDrivePower(0, 0);

            LogInfo(LogGroups.Console, "Finished");

        }


        void driveHandler(ICollection<DefaultUpdateResponseType> updates, ICollection<Fault> faults)
        {
            if (faults.Count > 0)
                LogError(LogGroups.Console, "Responses: " + updates.Count + " Faults: " + faults.Count);
            else
                LogInfo(LogGroups.Console, "Responses: " + updates.Count + " Faults: " + faults.Count);
        }

        Port<bool> countCompletePort = new Port<bool>();
        Port<int> counterPort = new Port<int>();
        private int requiredResponses;
        private int totalResponses;
        private int positives;
        private int negatives;

        void initCountHandler(int num)
        {
            requiredResponses = num;
            totalResponses = 0;
            positives = 0;
            negatives = 0;
            counterPort.Clear();
            Activate(Arbiter.Receive<int>(false, counterPort, countHandler));
        }

        void countHandler(int n)
        {
            // For debugging
//            LogInfo(LogGroups.Console, "Response: " + n);

            if (n >= 0)
                positives++;
            else
                negatives++;
            totalResponses++;
            if (totalResponses < requiredResponses)
                Activate(Arbiter.Receive<int>(false, counterPort, countHandler));
            else
            {
                if (negatives > 0)
                    countCompletePort.Post(false);
                else
                    countCompletePort.Post(true);
            }
        }

        #endregion

    }
}
